#pragma once

#include <memory>
#include <iostream>

namespace JAKADYN {
class JakaRobot;
}

class JAKADynIntf
{
public:
    enum JakaRobotType
    {
        MODEL_ZU = 0,
        MODEL_MC = 1,
        MODEL_ATOM = 2,
        MODEL_MAX = 3,
    };

private:
    std::shared_ptr<JAKADYN::JakaRobot> robot_intf_;

public:
    JAKADynIntf(JAKADynIntf::JakaRobotType type);
    virtual ~JAKADynIntf(){};

    /**
         * @brief 设置机器人动力学相关参数
         * @param dh_a  机器人MDH参数——a
         * @param dh_d  机器人MDH参数——d
         * @param dh_alpha  机器人MDH参数——alpha
         * @param dh_joint_offset  机器人MDH参数——joint_offset
         * @param dyn 机器人动力学参数
         * @param payload 机器人负载参数
         * @param rpy 机器人安装角度，r,p,y
         * @return int 0 for success, -1 for failure
        */
    int DynPara_init_with_full_dh(const double* payload,
                                  const double* rpy,
                                  const double* dyn,
                                  const double* link_dyn,
                                  const double* dh_a,
                                  const double* dh_d,
                                  const double* dh_alpha,
                                  const double* dh_joint_offset);

    /**
     * @brief 设置广义动量观测器滤波值
     * @param para  广义动量滤波器K值，数值越大，滤波约小
    */
    int setGMFilterPara(double para);

    /**
     * @brief 重置滤波器
     
    */
    int resetGm();

    /**
     * @brief 机器人关节转矩计算接口
     * @param q 关节角度
     * @param qd 关节速度
     * @param qdd 关节加速度
     * @param torque 输出计算值
    */
    int getTa(const double* q, const double* qd, const double* qdd, double* torque);

    /**
     * @brief 计算 摩擦力
     * @param q 关节角度
     * @param qd 关节速度
     * @param tg 输出计算值, 重力值
     * @param tf 输出计算值，摩擦力
    */
    int getJointGf(const double* lq, const double* qd, double* tg, double* tf);

    /**
     * @brief 计算 摩擦力
     * @param q 关节角度
     * @param qd 关节速度
     * @param tfc 输出计算值, 库伦摩擦力
     * @param tfv 输出计算值，粘滞摩擦力
    */
    int getJointF(const double* lq, const double* qd, double* tfc, double* tfv);

    /**
     * @brief 计算 广义动量观测值
     * @param q 关节角度
     * @param qd 关节速度
     * @param t 当前关节转矩
     * @param r_est 输出计算值，观测外力矩
    */
    int collisionDetect(const double* lq, const double* qd, const double* t, double* r_est);

    int getMqd(const double* q, const double* qd, double* t);
    int getSt(const double* q, const double* qd, double* t);
    int getMdqd(const double* q, const double* qd, double* torque);

    /****************
     *导纳控制相关接口
    *****************/

    int admittanceSynpos(const double* cartPos);

    /*************************--底座--*********************************************/
    int admittanceControlBFT(const double* cartPos,
                             const double* desired_CartPos,
                             const double* FT_sensor,
                             const int* opt,
                             const int* nortrak,
                             const double* FC,
                             const double* FT_user,
                             const double* FK,
                             double* FT_act,
                             double* new_cartPos,
                             const double* toolP);

    int admittanceInitBFT(const double* cartPos, const double* FT, double mass, const double* centroid);

    int getWbase(const double* q, const double* qd, const double* qdd, const double* toolParameters, double* wbase);

    int processFT(const double* FT, double* FTL);

    int update(double* newDynamic);
    /*************************--底座--*********************************************/

    int admittanceControl(const double* cartPos,
                          const double* desired_CartPos,
                          const double* FT_sensor,
                          const int* opt,
                          const int* nortrak,
                          const double* FC,
                          double* FT_user,
                          const double* FK,
                          double* FT_act,
                          double* new_cartPos,
                          const int motiontype);

    double get_true_singular_thresholds2(const double* jPos);

    int admittanceControlForCstForce(const double* cartPos,
                                     const double* desired_CartPos,
                                     const double* FT_sensor,
                                     const int* opt,
                                     const int* nortrak,
                                     const double* FC,
                                     double* FT_user,
                                     const double* FK,
                                     double* FT_act,
                                     double* new_cartPos,
                                     const int motiontype);

    int admittanceControlForDrag(const double* cartPos,
                                 const double* jPos,
                                 const double* desired_CartPos,
                                 const double* FT_sensor,
                                 const double* FT_body,
                                 const int* opt,
                                 const int* nortrak,
                                 const double* FC,
                                 double* FT_user,
                                 const double* FK,
                                 double* FT_act,
                                 double* new_cartPos,
                                 const int motiontype);

    int getSimulatedEndForce(const double* jPos, double* End_Force, double* jTorque, double* jTorque_ori, double* sensor_pos);

    int setEndSensorBias(const double* cartPos, const double* FT, double mass, const double* centroid);

    int initEndSensorBias(const double* bias);

    int getEndSensorBias(double* bias);

    unsigned int getAdmitDragStat();

    int updateEndSensorPayload(double mass, const double* centroid);

    int admittanceInit(const double* cartPos, const double* FT, double mass, const double* centroid);

    int get_Fact(const double* cartPos, const double* FT_sensor, double* FT_act, double mass, const double* centroid, const int bias_flag);

    int get_Fact_new(const double* sensor_cartPos, const double* FT_sensor, double* FT_act, const double* tcp_cartPos, const int ftframe);

    int SetDragDeadZone(const double* dead_zone);

    int setSensorCartPos(const double* cartPos);

    int setTcpOn(int flag);

    int setDragForceLimit(const double* force_limit);

    int setDragVelLimit(const double* vel_limit);

    int setBasicFrequency(const double* frequency);

    int resetAdmittanceVel();

    int setDragfilterSettings(const double* settings);

    int setUntouchVelLimit(const double* untouch_vel_limit);

    int setCstForceTolerance(const double* cst_force_tol);

    int setBodyFtObserveSensitivityLevel(const int sensitivitylevel);

    int getBodyFtObserveSensitivity(double* sensitivity);

    int setSingularSensitivityLevel(const int sensitivitylevel);

    int setDynJointMin(int joint_num, double min);

    int setDynJointMax(int joint_num, double max);

    int syncJacob(double* jpos);

    /****************
     *位置叠加相关接口
    *****************/

    // 设置低通滤波器截止频率
    int setSupperposLowPassFilterCultFrq(double cutFrq);
    // 设置滤波器类型，0 for 不滤波， 1 for 1阶低通滤波
    int setSuperposFilterType(int type);
    // 叠加接口
    int superpositionInWord(const double* cartPos, const double* desired_CartPos, const double* super_aim, int in_tcp_or_world, double* new_cartPos);
    // 清除滤波
    int resetSupperpos();
};